#pragma once

template<class K, class V, class Compare>
binary_heap_t<K, V, Compare>::binary_heap_t(Compare const & comp) :
  last_id_(-1), comp_(comp)
{
}

template<class K, class V, class Compare>
binary_heap_t<K, V, Compare>::binary_heap_t(size_type capacity,
                                            Compare const & comp) :
  last_id_(-1), comp_(comp)
{
  heap_data_.reserve(capacity);
  heap_index_.reserve(capacity);
}

template<class K, class V, class Compare>
heap_id binary_heap_t<K, V, Compare>::push(std::pair<K, V> const & el)
{
  return push(el.first, el.second);
}

template<class K, class V, class Compare>
heap_id binary_heap_t<K, V, Compare>::push(K key, V val)
{
  // Insert to a tree leaf
  heap_data_.push_back(details::binary_heap_el_t<K, V>(key, val, ++last_id_));
  // Update index
  heap_index_.insert(std::make_pair(last_id_, heap_data_.size() - 1));
  // Restore heap property
  bubble_up(heap_data_.size() - 1);

  return last_id_;
}

template<class K, class V, class Compare>
std::pair<K, V> binary_heap_t<K, V, Compare>::extract_top()
{
  assert(!heap_data_.empty());
  // Save the top
  std::pair<K, V> res(heap_data_[0].key, heap_data_[0].value);
  // Swap it with the last leaf
  swap_elements(0, heap_data_.size() - 1);
  // Delete the last leaf and an index of it
  heap_index_.erase(res.first);
  heap_data_.pop_back();
  // Restore heap property
  sift_down(0);
  return res;
}

template<class K, class V, class Compare>
void binary_heap_t<K, V, Compare>::swap_elements(
                                                 typename data_storage_t::size_type e1,
                                                 typename data_storage_t::size_type e2)
{
  assert(e1 < heap_data_.size());
  assert(e2 < heap_data_.size());
  std::swap(heap_data_[e1], heap_data_[e2]);
  // Update index
  heap_index_[heap_data_[e1].index] = e1;
  heap_index_[heap_data_[e2].index] = e2;
}

template<class K, class V, class Compare>
void binary_heap_t<K, V, Compare>::bubble_up(
                                             typename data_storage_t::size_type it)
{
  while (it != 0)
  {
    typename data_storage_t::size_type parent = node_parent(it);

    if (comp_(heap_data_[it].key, heap_data_[parent].key))
    {
      swap_elements(it, parent);
      it = parent;
    }
    else
    {
      break;
    }
  }
}

template<class K, class V, class Compare>
typename binary_heap_t<K, V, Compare>::data_storage_t::size_type binary_heap_t<
    K, V, Compare>::node_parent(typename data_storage_t::size_type it)
{
  return (it - 1) / 2;
}

template<class K, class V, class Compare>
typename binary_heap_t<K, V, Compare>::data_storage_t::size_type binary_heap_t<
    K, V, Compare>::node_lchild(typename data_storage_t::size_type it)
{
  return 2 * it + 1;
}

template<class K, class V, class Compare>
typename binary_heap_t<K, V, Compare>::data_storage_t::size_type binary_heap_t<
    K, V, Compare>::node_rchild(typename data_storage_t::size_type it)
{
  return 2 * it + 2;
}

template<class K, class V, class Compare>
void binary_heap_t<K, V, Compare>::sift_down(
                                             typename data_storage_t::size_type it)
{
  typename data_storage_t::size_type last_nonleaf =
      node_parent(heap_data_.size() - 1);

  while (it <= last_nonleaf)
  {
    typename data_storage_t::size_type lchild = node_lchild(it);
    typename data_storage_t::size_type rchild = node_rchild(it);
    typename data_storage_t::size_type min = it;
    if (lchild < heap_data_.size())
    {
      if (comp_(heap_data_[lchild].key, heap_data_[min].key))
      {
        min = lchild;
      }
    }
    if (rchild < heap_data_.size())
    {
      if (comp_(heap_data_[rchild].key, heap_data_[min].key))
      {
        min = rchild;
      }
    }

    if (min != it)
    {
      swap_elements(min, it);
      it = min;
    }
    else
    {
      break;
    }
  }
}

template<class K, class V, class Compare>
void binary_heap_t<K, V, Compare>::decrease_key(heap_id id, K new_key)
{
  typename index_storage_t::iterator it = heap_index_.find(id);
  if (it != heap_index_.end())
  {
    heap_data_[it->second].key = new_key;
    bubble_up(it->second);
  }
}
